import utime
from usr.libs.i2c import I2CInterface
from usr.libs.collections import Int
from usr.libs.logging import getLogger


logger = getLogger(__name__)


QMA6100P_CHIP_ID = 0x00
QMA6100P_XOUTL = 0x01
QMA6100P_XOUTH = 0x02
QMA6100P_YOUTL = 0x03
QMA6100P_YOUTH = 0x04
QMA6100P_ZOUTL = 0x05
QMA6100P_ZOUTH = 0x06
QMA6100P_STEP_CNT_L = 0x07
QMA6100P_STEP_CNT_M = 0x08
QMA6100P_STEP_CNT_H = 0x0D
QMA6100P_INT_STATUS_0 = 0x09
QMA6100P_INT_STATUS_1 = 0x0A
QMA6100P_INT_STATUS_2 = 0x0B
QMA6100P_INT_STATUS_3 = 0x0C
QMA6100P_FIFO_STATE = 0x0E
QMA6100P_REG_RANGE = 0x0F
QMA6100P_REG_BW_ODR = 0x10
QMA6100P_REG_POWER_MANAGE = 0x11
QMA6100P_STEP_SAMPLE_CNT = 0x12
QMA6100P_STEP_PRECISION = 0x13
QMA6100P_STEP_TIME_LOW = 0x14
QMA6100P_STEP_TIME_UP = 0x15
QMA6100P_INT_EN_0 = 0x16
QMA6100P_INT_EN_1 = 0x17
QMA6100P_INT_EN_2 = 0x18
QMA6100P_INT1_MAP_0 = 0x19
QMA6100P_INT1_MAP_1 = 0x1a
QMA6100P_INT2_MAP_0 = 0x1b
QMA6100P_INT2_MAP_1 = 0x1c
QMA6100P_INTPIN_CFG = 0x20
QMA6100P_INT_CFG = 0x21
QMA6100P_OS_CUST_X = 0x27
QMA6100P_OS_CUST_Y = 0x28
QMA6100P_OS_CUST_Z = 0x29
QMA6100P_REG_NVM = 0x33
QMA6100P_REG_RESET = 0x36
QMA6100P_DRDY_BIT = 0x10  # enable 1
QMA6100P_AMD_X_BIT = 0x01
QMA6100P_AMD_Y_BIT = 0x02
QMA6100P_AMD_Z_BIT = 0x04


class Qma6100pIntMap(object):
    QMA6100P_MAP_INT1 = 0
    QMA6100P_MAP_INT2 = 1
    QMA6100P_MAP_INT_NONE = 2


class Qma6100pBw(object):
    QMA6100P_BW_100 = 0
    QMA6100P_BW_200 = 1
    QMA6100P_BW_400 = 2
    QMA6100P_BW_800 = 3
    QMA6100P_BW_1600 = 4
    QMA6100P_BW_50 = 5
    QMA6100P_BW_25 = 6
    QMA6100P_BW_12_5 = 7
    QMA6100P_BW_OTHER = 8


class Qma6100pRange(object):
    QMA6100P_RANGE_2G = 0x01
    QMA6100P_RANGE_4G = 0x02
    QMA6100P_RANGE_8G = 0x04
    QMA6100P_RANGE_16G = 0x08
    QMA6100P_RANGE_32G = 0x0f


class Qma6100pNlpf(object):
    QMA6100P_LPF_OFF = 0x00 << 5
    QMA6100P_LPF_1 = 0x04 << 5
    QMA6100P_LPF_2 = 0x01 << 5
    QMA6100P_LPF_4 = 0x02 << 5
    QMA6100P_LPF_8 = 0x03 << 5
    QMA6100P_LPF_RESERVED = 0xff


class Qma6100pNhpf(object):
    QMA6100P_HPF_DIV_OFF = 0x00 << 5
    QMA6100P_HPF_DIV_10 = 0x01 << 5
    QMA6100P_HPF_DIV_25 = 0x02 << 5
    QMA6100P_HPF_DIV_50 = 0x03 << 5
    QMA6100P_HPF_DIV_100 = 0x04 << 5
    QMA6100P_HPF_DIV_200 = 0x05 << 5
    QMA6100P_HPF_DIV_400 = 0x06 << 5
    QMA6100P_HPF_DIV_800 = 0x07 << 5
    QMA6100P_HPF_RESERVED = 0xff


class Qma6100pMode(object):
    QMA6100P_MODE_STANDBY = 0
    QMA6100P_MODE_ACTIVE = 1
    QMA6100P_MODE_MAX = 2


class Qma6100pMclk(object):
    QMA6100P_MCLK_102_4K = 0x03
    QMA6100P_MCLK_51_2K = 0x04
    QMA6100P_MCLK_25_6K = 0x05
    QMA6100P_MCLK_12_8K = 0x06
    QMA6100P_MCLK_6_4K = 0x07
    QMA6100P_MCLK_RESERVED = 0xff


class Qma6100pStepLpf(object):
    QMA6100P_STEP_LPF_0 = 0x00 << 6
    QMA6100P_STEP_LPF_2 = 0x01 << 6
    QMA6100P_STEP_LPF_4 = 0x02 << 6
    QMA6100P_STEP_LPF_8 = 0x03 << 6
    QMA6100P_STEP_LPF_RESERVED = 0xff


class Qma6100pStepAxis(object):
    QMA6100P_STEP_AXIS_ALL = 0x00
    QMA6100P_STEP_AXIS_YZ = 0x01
    QMA6100P_STEP_AXIS_XZ = 0x02
    QMA6100P_STEP_AXIS_XY = 0x03
    QMA6100P_STEP_AXIS_RESERVED = 0xff


class Qma6100pStepStartCnt(object):
    QMA6100P_STEP_START_0 = 0x00
    QMA6100P_STEP_START_4 = 0x20
    QMA6100P_STEP_START_8 = 0x40
    QMA6100P_STEP_START_12 = 0x60
    QMA6100P_STEP_START_16 = 0x80
    QMA6100P_STEP_START_24 = 0xa0
    QMA6100P_STEP_START_32 = 0xc0
    QMA6100P_STEP_START_40 = 0xe0
    QMA6100P_STEP_START_RESERVED = 0xff


class Qma6100pFifoMode(object):
    QMA6100P_FIFO_MODE_NONE = 0
    QMA6100P_FIFO_MODE_FIFO = 1
    QMA6100P_FIFO_MODE_STREAM = 2
    QMA6100P_FIFO_MODE_BYPASS = 3
    QMA6100P_FIFO_MODE_MAX = 4


class Qma6100pTap(object):
    QMA6100P_TAP_SINGLE = 0x80
    QMA6100P_TAP_DOUBLE = 0x20
    QMA6100P_TAP_TRIPLE = 0x10
    QMA6100P_TAP_QUARTER = 0x01
    QMA6100P_TAP_MAX = 0xff


class Qma6100p(I2CInterface):
    def __init__(self, i2c, slave_addr=0x12):
        self.slave_addr = slave_addr
        self.g_per_lsb = None
        self.step_last = 0
        super().__init__(i2c, self.slave_addr)

    def __str__(self):
        return '{}'.format(type(self).__name__)

    def readReg(self, addr, size=1, delay=0):
        data = self.read(bytes([addr]), size=size, delay=delay)
        if size == 1:
            return data[0]
        else:
            return data

    def writeReg(self, addr, data):
        self.write(bytes([addr]), bytes([data]))

    @staticmethod
    def delay(delay):
        utime.sleep_us(delay)

    def getChipId(self):
        return self.readReg(QMA6100P_CHIP_ID)

    def softReset(self):
        logger.debug("{} soft_reset".format(self))
        self.writeReg(QMA6100P_REG_RESET, 0xb6)
        self.delay(5)
        self.writeReg(QMA6100P_REG_RESET, 0x00)
        self.delay(10)

        for retry in range(100):
            reg_0x33 = self.readReg(QMA6100P_REG_NVM)
            logger.debug("confirm-{} read 0x33 = 0x{:02X}".format(retry, reg_0x33))
            if reg_0x33 & 0x01 and reg_0x33 & 0x04:
                break
            self.delay(2)

    def setRange(self, reg_data):
        if reg_data == Qma6100pRange.QMA6100P_RANGE_4G:  # 488ug/LSB
            self.g_per_lsb = 488 / (10 ** 6)
        elif reg_data == Qma6100pRange.QMA6100P_RANGE_8G:  # 977ug/LSB
            self.g_per_lsb = 977 / (10 ** 6)
        elif reg_data == Qma6100pRange.QMA6100P_RANGE_16G:  # 1.95mg/LSB
            self.g_per_lsb = 1.95 / 1000
        elif reg_data == Qma6100pRange.QMA6100P_RANGE_32G:  # 3.91mg/LSB
            self.g_per_lsb = 3.91 / 1000
        else:
            self.g_per_lsb = 244 / (10 ** 6)  # 244ug/LSB
        self.writeReg(QMA6100P_REG_RANGE, reg_data)

    def setBw(self, bw):
        self.writeReg(QMA6100P_REG_BW_ODR, bw | Qma6100pNlpf.QMA6100P_LPF_OFF)

    def active(self):
        self.writeReg(QMA6100P_REG_POWER_MANAGE, Qma6100pMclk.QMA6100P_MCLK_51_2K | 0x80)

    def standby(self):
        self.writeReg(QMA6100P_REG_POWER_MANAGE, 0x00)

    def setDrdyConfig(self, enable, int_map=Qma6100pIntMap.QMA6100P_MAP_INT1):
        logger.debug("set_drdy_config {} {}".format(enable, int_map))

        if int_map == Qma6100pIntMap.QMA6100P_MAP_INT1:
            reg_int_map = QMA6100P_INT1_MAP_1
        elif int_map == Qma6100pIntMap.QMA6100P_MAP_INT2:
            reg_int_map = QMA6100P_INT2_MAP_1
        else:
            raise ValueError('`int_map` not valid: {}'.format(int_map))

        data_enable = self.readReg(QMA6100P_INT_EN_1)
        data_int_map = self.readReg(reg_int_map)

        if enable:
            data_enable |= QMA6100P_DRDY_BIT
            data_int_map |= QMA6100P_DRDY_BIT
        else:
            data_enable &= (~QMA6100P_DRDY_BIT)
            data_int_map &= (~QMA6100P_DRDY_BIT)

        self.writeReg(QMA6100P_INT_EN_1, data_enable)
        self.writeReg(reg_int_map, data_int_map)

    def getStepCounter(self):
        try:
            data_low = self.readReg(QMA6100P_STEP_CNT_L, size=2)
            data_high = self.readReg(QMA6100P_STEP_CNT_H)
        except Exception as e:
            raise ValueError('get_stepcounter data error: {}'.format(e))

        step_num = ((data_high << 16) | (data_low[1] << 8) | data_low[0])
        step_diff = step_num - self.step_last
        if abs(step_diff) > 100:
            step_num_temp = []
            for i in range(3):
                data_low = self.readReg(QMA6100P_STEP_CNT_L, size=2)
                data_high = self.readReg(QMA6100P_STEP_CNT_H)
                step_num_temp.append((data_high << 16) | (data_low[1] << 8) | data_low[0])
                self.delay(2)
            if step_num_temp[0] == step_num_temp[1] == step_num_temp[2]:
                step_num = step_num_temp[0]
            else:
                raise ValueError('get_stepcounter confirm error')

        self.step_last = step_num
        return step_num, step_diff  # （总量，增量）

    def setStepCounterConfig(self, enable):
        logger.debug("set_stepcounter_config {}".format(enable))
        odr = 100  # 75
        self.writeReg(0x13, 0x80)  # clear step
        self.step_last = 0
        self.delay(1)

        if enable:
            self.writeReg(0x12, 0x8f)  # old 0x94
        else:
            self.writeReg(0x12, 0x00)

        self.writeReg(0x13, 0x7f)

        # odr 100 Hz, 10ms
        reg_14 = (280 * odr) // 1000  # about:280 ms
        reg_15 = ((2000 // 8) * odr) // 1000  # 2000 ms
        logger.debug("step time config 0x14=0x{:02X} 0x15=0x{:02X}".format(reg_14, reg_15))

        self.writeReg(0x14, reg_14)
        self.writeReg(0x15, reg_15)

        reg_1e = self.readReg(0x1e)
        reg_1e &= 0x3f
        self.writeReg(0x1e, reg_1e | Qma6100pStepLpf.QMA6100P_STEP_LPF_2)  # default 0x08
        # start count, p2p, fix peak
        self.writeReg(0x1f, Qma6100pStepStartCnt.QMA6100P_STEP_START_24 | 0x10)
        # select axis
        reg_32 = self.readReg(0x32)
        reg_32 &= 0xfc
        reg_32 |= Qma6100pStepAxis.QMA6100P_STEP_AXIS_XY
        self.writeReg(0x32, reg_32)

    def setStepInterruptConfig(self, enable, int_map=Qma6100pIntMap.QMA6100P_MAP_INT1):
        logger.debug("setStepInterruptConfig {}".format(enable))

        reg_16 = self.readReg(0x16)
        reg_19 = self.readReg(0x19)
        reg_1b = self.readReg(0x1b)

        if enable:
            reg_16 |= 0x08
            reg_19 |= 0x08
            reg_1b |= 0x08
            self.writeReg(0x16, reg_16)
            if int_map == Qma6100pIntMap.QMA6100P_MAP_INT1:
                self.writeReg(0x19, reg_19)
            elif int_map == Qma6100pIntMap.QMA6100P_MAP_INT2:
                self.writeReg(0x1b, reg_1b)
            else:
                raise ValueError('unknown int_map: {}'.format(int_map))
        else:
            reg_16 &= (~0x08)
            reg_19 &= (~0x08)
            reg_1b &= (~0x08)
            self.writeReg(0x16, reg_16)
            self.writeReg(0x19, reg_19)
            self.writeReg(0x1b, reg_1b)

    def setSigstepInterruptConfig(self, enable, int_map=Qma6100pIntMap.QMA6100P_MAP_INT1):
        logger.debug("set_sigstep_int_config {} {}".format(enable, int_map))

        reg_16 = self.readReg(0x16)
        reg_19 = self.readReg(0x19)
        reg_1b = self.readReg(0x1b)
        self.writeReg(0x1d, 0x1a)

        if enable:
            reg_16 |= 0x40
            reg_19 |= 0x40
            reg_1b |= 0x40
            self.writeReg(0x16, reg_16)
            if int_map == Qma6100pIntMap.QMA6100P_MAP_INT1:
                self.writeReg(0x19, reg_19)
            elif int_map == Qma6100pIntMap.QMA6100P_MAP_INT2:
                self.writeReg(0x1b, reg_1b)
            else:
                raise ValueError('unknown int_map: {}'.format(int_map))
        else:
            reg_16 &= (~0x40)
            reg_19 &= (~0x40)
            reg_1b &= (~0x40)
            self.writeReg(0x16, reg_16)
            self.writeReg(0x19, reg_19)
            self.writeReg(0x1b, reg_1b)

    def setAnymotionConfig(self, enable, int_map=Qma6100pIntMap.QMA6100P_MAP_INT1, sigmotion=False):
        logger.debug("setAnymotionConfig {}, sig_motion: {}".format(enable, sigmotion))

        if int_map == Qma6100pIntMap.QMA6100P_MAP_INT1:
            reg_int_map = QMA6100P_INT1_MAP_1
        elif int_map == Qma6100pIntMap.QMA6100P_MAP_INT2:
            reg_int_map = QMA6100P_INT2_MAP_1
        else:
            raise ValueError('`int_map` is not valid: {}'.format(int_map))

        data_enable = self.readReg(QMA6100P_INT_EN_2)
        data_int_map = self.readReg(reg_int_map)
        reg_0x2c = self.readReg(0x2c)
        reg_0x2f = self.readReg(0x2f)
        reg_0x30 = self.readReg(0x30)

        reg_0x2c |= 0x00  # (ANY_MOT_DUR<1:0> + 1)samples
        reg_0x2f &= (~0x40)  # bit6 ANY_MOT_IN_SEL, 0 : Any-motion Input is Slope. 1 : Any-motion Input is Acceleration
        reg_0x30 |= 0x80  # add by yang, tep counter, raise wake, and tap detector,any motion by pass LPF

        self.writeReg(0x2c, reg_0x2c)  # ANY_MOT_DUR[1:0]
        self.writeReg(0x2e, 0x10)  # 0.488*16*32 = 250mg
        self.writeReg(0x30, reg_0x30)  # default 0x3f

        # add by yang, tep counter, raise wake, and tap detector,any motion by pass LPF
        if enable:
            data_enable |= 0x07
            data_int_map |= 0x01
        else:
            data_enable &= (~0x07)
            data_int_map &= (~0x01)

        self.writeReg(QMA6100P_INT_EN_2, data_enable)
        self.writeReg(reg_int_map, data_int_map)

        if not sigmotion:
            reg_0x2f &= (~0x01)  # bit0: select any motion
        else:
            reg_0x2f |= 0x01  # bit0: select significant motion
            if int_map == Qma6100pIntMap.QMA6100P_MAP_INT1:
                reg_int_map = QMA6100P_INT1_MAP_0
            elif int_map == Qma6100pIntMap.QMA6100P_MAP_INT2:
                reg_int_map = QMA6100P_INT2_MAP_0
            else:
                raise ValueError('`int_map` is not valid: {}'.format(int_map))
            data_int_map = self.readReg(reg_int_map)
            data_int_map = data_int_map | 0x01 if enable else data_int_map & (~0x01)
            self.writeReg(0x2f, reg_0x2f)
            self.writeReg(reg_int_map, data_int_map)

        self.writeReg(0x2f, reg_0x2f)

    def setNomotionConfig(self, enable, seconds=10, int_map=Qma6100pIntMap.QMA6100P_MAP_INT1):
        logger.debug("set_nomotion_config {}".format(enable))

        if int_map == Qma6100pIntMap.QMA6100P_MAP_INT1:
            reg_int_map = QMA6100P_INT1_MAP_1
        elif int_map == Qma6100pIntMap.QMA6100P_MAP_INT2:
            reg_int_map = QMA6100P_INT2_MAP_1
        else:
            reg_int_map = 0x00

        data_enable = self.readReg(QMA6100P_INT_EN_2)
        data_int_map = self.readReg(reg_int_map)
        reg_0x2c = self.readReg(0x2c)

        # calc no-notion duration reg
        if seconds <= 16:
            reg_0x2c |= (((seconds - 1) << 2) & 0x3f)
        elif seconds <= 95:
            if seconds < 20:
                seconds = 20
            seconds = seconds // 5
            reg_0x2c |= (((seconds - 4) << 2) | 0x40)
        else:
            if seconds < 100:
                seconds = 100
            seconds = seconds // 10
            reg_0x2c |= (((seconds - 10) << 2) | 0x80)

        # calc no-notion duration reg
        self.writeReg(0x2c, reg_0x2c)
        self.writeReg(0x2d, 0x14)  # TH= NO_MOT_TH[7:0] * 16 * LSB

        if enable:
            data_enable |= 0xe0
            data_int_map |= 0x80
        else:
            data_enable &= (~0xe0)
            data_int_map &= (~0x80)

        self.writeReg(QMA6100P_INT_EN_2, data_enable)
        self.writeReg(reg_int_map, data_int_map)

    def setTapConfig(self, enable, tap_type=Qma6100pTap.QMA6100P_TAP_SINGLE, int_map=Qma6100pIntMap.QMA6100P_MAP_INT1):
        logger.debug("setTapConfig {}".format(enable))

        tap_reg_en = tap_type
        tap_reg_int = tap_type & 0xfe

        if int_map == Qma6100pIntMap.QMA6100P_MAP_INT1:
            if tap_type == Qma6100pTap.QMA6100P_TAP_QUARTER:
                reg_int_map = QMA6100P_INT1_MAP_1
            else:
                reg_int_map = QMA6100P_INT1_MAP_0
        elif int_map == Qma6100pIntMap.QMA6100P_MAP_INT2:
            if tap_type == Qma6100pTap.QMA6100P_TAP_QUARTER:
                reg_int_map = QMA6100P_INT2_MAP_1
            else:
                reg_int_map = QMA6100P_INT2_MAP_0
        else:
            reg_int_map = 0x00

        data_enable = self.readReg(QMA6100P_INT_EN_0)
        data_int_map = self.readReg(reg_int_map)
        reg_1e = self.readReg(0x1e)
        reg_30 = self.readReg(0x30)

        reg_1e |= 0x06
        self.writeReg(0x1e, reg_1e)  # TAP_QUIET_TH 31.25*8 = 250mg
        self.writeReg(0x2a, 0x86)  # 0x85 tap config1
        self.writeReg(0x2b, 0xc0 + 6)  # tap config2
        # add by yang, step counter, raise wake, and tap detector,any motion by pass LPF
        reg_30 |= 0x80 | 0x40
        self.writeReg(0x30, reg_30)  # default 0x3f
        # add by yang, tep counter, raise wake, and tap detector,any motion by pass LPF

        if enable:
            data_enable |= tap_reg_en
            if tap_type == Qma6100pTap.QMA6100P_TAP_QUARTER:
                data_int_map |= 0x02
            else:
                data_int_map |= tap_reg_int
        else:
            data_enable &= (~tap_reg_en)
            if tap_type == Qma6100pTap.QMA6100P_TAP_QUARTER:
                data_int_map &= (~0x02)
            else:
                data_int_map &= (~tap_reg_int)

        self.writeReg(QMA6100P_INT_EN_0, data_enable)
        self.writeReg(reg_int_map, data_int_map)

    def setRaiseConfig(self, enable, layout=3, int_map=Qma6100pIntMap.QMA6100P_MAP_INT1):
        logger.debug("setRaiseConfig {}".format(enable))

        if layout % 2:
            reg_0x42 = self.readReg(0x42)
            reg_0x42 |= 0x80  # 0x42 bit 7 swap x and y
            self.writeReg(0x42, reg_0x42)
        else:
            reg_0x42 = self.readReg(0x42)
            reg_0x42 &= 0x7f  # 0x42 bit 7 swap x and y
            self.writeReg(0x42, reg_0x42)

        reg_0x42 = self.readReg(0x42)
        logger.debug("raise_config 0x42 = 0x{:02X}".format(reg_0x42))

        yz_th_sel = 4
        y_th = -3 // -2  # -16 ~ 15
        x_th = 6  # 0--7.5
        z_th = 6  # -8--7

        if 0 <= layout <= 3:
            z_th = 3
            if layout == 2 or layout == 3:
                y_th = 3
            elif layout == 0 or layout == 1:
                y_th = -3
        elif 4 <= layout <= 7:
            z_th = -3
            if layout == 6 or layout == 7:
                y_th = 3
            elif layout == 4 or layout == 5:
                y_th = -3
        else:
            pass

        # 0x34 YZ_TH_SEL[7:5]	Y_TH[4:0], default 0x9d  (YZ_TH_SEL   4   9.0 m/s2 | Y_TH  -3  -3 m/s2)
        # qmaX981_write_reg(0x34, 0x9d);	//|yz|>8 m/s2, y>-3 m/m2
        reg_0x34 = 0x00
        if y_th & 0x80:
            reg_0x34 |= yz_th_sel << 5
            reg_0x34 |= (y_th & 0x0f) | 0x10
            self.writeReg(0x34, reg_0x34)
        else:
            reg_0x34 |= yz_th_sel << 5
            reg_0x34 |= y_th
            self.writeReg(0x34, reg_0x34)  # |yz|>8m/s2, y<3 m/m2

        # Z_TH<7:4>: -8~7, LSB 1 (unit : m/s2)	X_TH<3:0>: 0~7.5, LSB 0.5 (unit : m/s2)
        # qmaX981_write_reg(0x1e, 0x68);	//6 m/s2, 4 m/m2
        self.writeReg(0x22, (0x19 | (0x03 << 6)))  # 12m/s2 , 0.5m/s2
        self.writeReg(0x23, (0x7c | (0x03 >> 2)))
        # qmaX981_write_reg(0x22, (0x19|(0x02<<6)));			// 12m/s2 , 0.5m/s2
        # qmaX981_write_reg(0x23, (0x7c|(0x02)));

        reg_0x35 = 0x00
        if z_th & 0x80:
            reg_0x35 |= (x_th & 0x0f)
            reg_0x35 |= ((z_th << 4) | 0x80)
            self.writeReg(0x35, reg_0x35)
        else:
            reg_0x35 |= (x_th & 0x0f)
            reg_0x35 |= (z_th << 4)
            self.writeReg(0x35, reg_0x35)

        # RAISE_WAKE_PERIOD*(1/ODR), default 0x81
        self.writeReg(0x25, 0x50)
        # add by yang, tep counter, raise wake, and tap detector,any motion by pass LPF
        reg_0x30 = self.readReg(0x30)
        self.writeReg(0x30, reg_0x30 | 0x40 | 0x3f)  # default 0x3f
        # add by yang, tep counter, raise wake, and tap detector,any motion by pass LPF

        reg_16 = self.readReg(0x16)
        reg_19 = self.readReg(0x19)
        reg_1b = self.readReg(0x1b)

        # 0x24: RAISE_WAKE_TIMEOUT_TH<7:0>: Raise_wake_timeout_th[11:0] * ODR period = timeout count
        # 0x25: RAISE_WAKE_PERIOD<7:0>: Raise_wake_period[10:0] * ODR period = wake count
        # 0x26:
        # RAISE_MODE: 0:raise wake function, 1:ear-in function
        # RAISE_WAKE_PERIOD<10:8>: Raise_wake_period[10:0] * ODR period = wake count
        # RAISE_WAKE_TIMEOUT_TH<11:8>: Raise_wake_timeout_th[11:0] * ODR period = timeout count

        if enable:
            reg_16 |= (0x02 | 0x04)
            reg_19 |= (0x02 | 0x04)
            reg_1b |= (0x02 | 0x04)
            self.writeReg(0x16, reg_16)
            if int_map == Qma6100pIntMap.QMA6100P_MAP_INT1:
                self.writeReg(0x19, reg_19)
            elif int_map == Qma6100pIntMap.QMA6100P_MAP_INT2:
                self.writeReg(0x1b, reg_1b)
            else:
                raise ValueError('`int_map` is not valid: {}'.format(int_map))
        else:
            reg_16 &= ~(0x02 | 0x04)
            reg_19 &= ~(0x02 | 0x04)
            reg_1b &= ~(0x02 | 0x04)
            self.writeReg(0x16, reg_16)
            self.writeReg(0x19, reg_19)
            self.writeReg(0x1b, reg_1b)

    def dumpReg(self):
        reg_map = [
            0x0f, 0x10, 0x11, 0x17, 0x18, 0x1a, 0x1c,
            0x20, 0x43, 0x45, 0x4a, 0x50, 0x56, 0x57
        ]
        logger.debug("-----------dump_reg-----------")
        for reg in reg_map:
            reg_data = self.readReg(reg)
            logger.debug("0x{:02X} = 0x{:02X}".format(reg, reg_data))

    def init(self):
        chip_id = self.getChipId()
        if chip_id != 0x90:
            raise ValueError('read wrong chip id: {}'.format(chip_id))
        self.soft_reset()
        self.writeReg(QMA6100P_REG_POWER_MANAGE, 0x84)
        self.writeReg(0x4a, 0x20)
        self.writeReg(0x56, 0x01)
        self.writeReg(0x5f, 0x80)
        self.delay(2)
        self.writeReg(0x5f, 0x00)
        self.delay(10)
        self.writeReg(0x21, 0x03)  # latch mode
        self.set_range(Qma6100pRange.QMA6100P_RANGE_2G)
        self.set_bw(Qma6100pBw.QMA6100P_BW_100)

    def setFifoConfig(self, enable, fifo_mode=Qma6100pFifoMode.QMA6100P_FIFO_MODE_STREAM, wmk=10, int_map=Qma6100pIntMap.QMA6100P_MAP_INT1):
        logger.debug("setFifoConfig mode: {} enable: {}".format(fifo_mode, enable))
        if int_map == Qma6100pIntMap.QMA6100P_MAP_INT1:
            reg_int_map = QMA6100P_INT1_MAP_1
        elif int_map == Qma6100pIntMap.QMA6100P_MAP_INT2:
            reg_int_map = QMA6100P_INT2_MAP_1
        else:
            raise ValueError('`int_map` not valid: {}'.format(int_map))

        data_enable = self.readReg(QMA6100P_INT_EN_1)
        data_int_map = self.readReg(reg_int_map)
        reg_21 = self.readReg(0x21)  # fifo use latch int
        reg_3e = self.readReg(0x3e)

        self.writeReg(0x21, reg_21 | 0x01)
        if enable:
            if fifo_mode == Qma6100pFifoMode.QMA6100P_FIFO_MODE_FIFO:
                reg_3e |= 0x47
                self.writeReg(0x31, wmk)  # max 0x40
                self.writeReg(0x3E, reg_3e)  # bit[6:7] 0x00:BYPASS 0x40:FIFO 0x80:STREAM
                self.writeReg(QMA6100P_INT_EN_1, data_enable | 0x20)
                self.writeReg(reg_int_map, data_int_map | 0x20)
            elif fifo_mode == Qma6100pFifoMode.QMA6100P_FIFO_MODE_STREAM:
                reg_3e |= 0x87
                self.writeReg(0x31, wmk)  # 0x3f
                self.writeReg(0x3E, reg_3e)  # bit[6:7] 0x00:BYPASS 0x40:FIFO 0x80:STREAM
                self.writeReg(QMA6100P_INT_EN_1, data_enable | 0x40)
                self.writeReg(reg_int_map, data_int_map | 0x40)
            elif fifo_mode == Qma6100pFifoMode.QMA6100P_FIFO_MODE_BYPASS:
                reg_3e |= 0x07
                self.writeReg(0x3E, reg_3e)  # bit[6:7] 0x00:BYPASS 0x40:FIFO 0x80:STREAM
                self.writeReg(QMA6100P_INT_EN_1, data_enable | 0x20)
                self.writeReg(reg_int_map, data_int_map | 0x20)
            else:
                raise ValueError('unknown fifo_mode: {}'.format(fifo_mode))
        else:
            data_enable &= (~0x60)
            data_int_map &= (~0x60)
            self.writeReg(QMA6100P_INT_EN_1, data_enable)
            self.writeReg(reg_int_map, data_int_map)

    def getFifo(self):
        fifo_mode = self.readReg(0x3E) >> 6
        try:
            databuf_len = self.readReg(QMA6100P_FIFO_STATE)
        except Exception as e:
            logger.debug("read_fifo state error: {}".format(e))
            return 0

        fifo_len = databuf_len & 0x7f
        if fifo_len > 64:
            logger.debug("read_fifo depth({}) error".format(fifo_len))
            return 0

        fifo_buf = bytearray()
        for _ in range(fifo_len):
            fifo_buf += self.readReg(0x3f, size=6)

        if fifo_mode == Qma6100pFifoMode.QMA6100P_FIFO_MODE_FIFO:
            self.writeReg(0x3e, 0x47)
        elif fifo_mode == Qma6100pFifoMode.QMA6100P_FIFO_MODE_STREAM:
            self.writeReg(0x3e, 0x87)
        elif fifo_mode == Qma6100pFifoMode.QMA6100P_FIFO_MODE_BYPASS:
            self.writeReg(0x3e, 0x07)
        else:
            raise ValueError('unknown fifo_mode: {}'.format(fifo_mode))

        return fifo_buf, fifo_len

    def exeFifo(self, fifo_buf, fifo_len):
        raw_data = bytearray(3)
        logger.debug("fifo_depth = {}".format(fifo_len))
        for icount in range(fifo_len):
            raw_data[0] = ((fifo_buf[1 + icount * 6] << 8) | fifo_buf[0 + icount * 6]) >> 2
            raw_data[1] = ((fifo_buf[3 + icount * 6] << 8) | fifo_buf[2 + icount * 6]) >> 2
            raw_data[2] = ((fifo_buf[5 + icount * 6] << 8) | fifo_buf[4 + icount * 6]) >> 2
            logger.debug("{}: [0x{:02X}, 0x{:02X}, 0x{:02X}]".format(icount, raw_data[0], raw_data[1], raw_data[2]))

    def irqHdlr(self):
        for retry in range(10):
            try:
                databuf = self.readReg(QMA6100P_INT_STATUS_0, size=4)
            except Exception as e:
                logger.debug(e)
                continue
            else:
                break
        else:
            logger.debug("irq_hdlr read status fail!")
            return
        # logger.debug('-------------irq_hdlr---------------')
        # logger.debug('0x09: {:08b}'.format(databuf[0]))
        # logger.debug('0x0A: {:08b}'.format(databuf[1]))
        # logger.debug('0x0B: {:08b}'.format(databuf[1]))
        # logger.debug('0x0C: {:08b}'.format(databuf[2]))
        return databuf

    def getRawXYZ(self):
        if getattr(self, 'drdy_int', False):
            for i in range(3):
                try:
                    drdy = self.readReg(QMA6100P_INT_STATUS_2)
                except Exception as e:
                    logger.debug(e)
                else:
                    if drdy & 0x10:
                        break
                self.delay(1)
            else:
                drdy = 0x00
        else:
            drdy = 0x10

        if drdy & 0x10:
            try:
                databuf = self.readReg(QMA6100P_XOUTL, size=6)
            except Exception as e:
                logger.debug(e)
                return
            else:
                return (
                    ((databuf[1] << 8) | databuf[0]) >> 2,
                    ((databuf[3] << 8) | databuf[2]) >> 2,
                    ((databuf[5] << 8) | databuf[4]) >> 2
                )

    def getAccXYZ(self):
        rawData = self.get_raw_xyz()
        if rawData:
            return (
                Int.from_bytes(rawData[0:1]) * self.g_per_lsb,
                Int.from_bytes(rawData[1:2]) * self.g_per_lsb,
                Int.from_bytes(rawData[2:3]) * self.g_per_lsb
            )
